#include "bsp.h"

volatile tBNO055_powermode BNO055_powermode;
volatile tBNO055_opmode BNO055_opmode;
volatile tBNO055_Data BNO055_Data;
#define BNO055_ADDRESS 0x50

void MyI2C_W_SCL(uint8_t BitValue)
{
    HAL_GPIO_WritePin(GPIOB, i2c1_scl_Pin, (GPIO_PinState)BitValue);
    delay_us(10);
}

void MyI2C_W_SDA(uint8_t BitValue)
{
    HAL_GPIO_WritePin(GPIOB, i2c1_sda_Pin, (GPIO_PinState)BitValue);
    delay_us(10);
}

uint8_t MyI2C_R_SDA(void)
{
    uint8_t BitValue;
    BitValue = HAL_GPIO_ReadPin(GPIOB, i2c1_sda_Pin);
    delay_us(10);
    return BitValue;
}

void MyI2C_Start(void)
{
    MyI2C_W_SDA(1);
    MyI2C_W_SCL(1);
    MyI2C_W_SDA(0);
    MyI2C_W_SCL(0);
}

void MyI2C_Stop(void)
{
    MyI2C_W_SDA(0);
    MyI2C_W_SCL(1);
    MyI2C_W_SDA(1);
}

void MyI2C_SendByte(uint8_t Byte)
{
    uint8_t i;

    for (i = 0; i < 8 ; i++) {
        MyI2C_W_SDA(Byte & (0x80 >> i));
        MyI2C_W_SCL(1);
        MyI2C_W_SCL(0);
    }
}

uint8_t MyI2C_ReceiveByte(void)
{
    uint8_t i, Byte = 0x00;
    MyI2C_W_SDA(1);

    for (i = 0; i < 8; i++) {
        MyI2C_W_SCL(1);

        if (MyI2C_R_SDA() == 1) {
            Byte |= (0x80 >> i);
        }

        MyI2C_W_SCL(0);
    }

    return Byte;
}

void MyI2C_SendAck(uint8_t AckBit)
{
    MyI2C_W_SDA(AckBit);
    MyI2C_W_SCL(1);
    MyI2C_W_SCL(0);
}

uint8_t MyI2C_ReceiveAck(void)
{
    uint8_t AckBit;
    MyI2C_W_SDA(1);
    MyI2C_W_SCL(1);
    AckBit = MyI2C_R_SDA();
    MyI2C_W_SCL(0);
    return AckBit;
}

void BNO055_WriteReg(uint8_t RegAddress, uint8_t Data)
{
		MyI2C_Start();
		MyI2C_SendByte(BNO055_ADDRESS);
		MyI2C_ReceiveAck();
		MyI2C_SendByte(RegAddress);
		MyI2C_ReceiveAck();
		MyI2C_SendByte(Data);
		MyI2C_ReceiveAck();	
		MyI2C_Stop();
}

uint8_t BNO055_ReadReg(uint8_t RegAddress)
{
		uint8_t Data;
	
		MyI2C_Start();
		MyI2C_SendByte(0x50);
		MyI2C_ReceiveAck();
		MyI2C_SendByte(RegAddress);
		MyI2C_ReceiveAck();
	
		MyI2C_Start();
		MyI2C_SendByte(0x51);
		MyI2C_ReceiveAck();
		Data = MyI2C_ReceiveByte();
		MyI2C_SendAck(1);
		MyI2C_Stop();
	
		return Data;
}

uint8_t  BNO055_Read_ID(void)
{
		uint8_t ID = BNO055_ReadReg(0x00);
//	ID = (ID == 0xA0) ? 1 : 2;
		return ID;
}


void BNO055_Init(void)
{
		BNO055_WriteReg(BNO055_PWR_MGMT_1, 0x01);
		BNO055_WriteReg(BNO055_PWR_MGMT_2, 0x00);
		BNO055_WriteReg(BNO055_SMPLRT_DIV, 0x09);
		BNO055_WriteReg(BNO055_CONFIG, 0x06);
		BNO055_WriteReg(BNO055_GYRO_CONFIG, 0x18);
		BNO055_WriteReg(BNO055_ACCEL_CONFIG, 0x18);
}

void BNO055_GetData1(void)
{
	uint8_t DataH, DataL;
	
	DataH = BNO055_ReadReg(BNO055_ACCEL_XOUT_H);
	DataL = BNO055_ReadReg(BNO055_ACCEL_XOUT_L);
	BNO055_Data.AccX = (DataH << 8) | DataL;
	
	DataH = BNO055_ReadReg(BNO055_ACCEL_YOUT_H);
	DataL = BNO055_ReadReg(BNO055_ACCEL_YOUT_L);
	BNO055_Data.AccY = (DataH << 8) | DataL;

	DataH = BNO055_ReadReg(BNO055_ACCEL_ZOUT_H);
	DataL = BNO055_ReadReg(BNO055_ACCEL_ZOUT_L);
	BNO055_Data.AccZ = (DataH << 8) | DataL;
	
	DataH = BNO055_ReadReg(BNO055_GYRO_XOUT_H);
	DataL = BNO055_ReadReg(BNO055_GYRO_XOUT_L);
	BNO055_Data.GyroX = (DataH << 8) | DataL;
	
	DataH = BNO055_ReadReg(BNO055_GYRO_YOUT_H);
	DataL = BNO055_ReadReg(BNO055_GYRO_YOUT_L);
	BNO055_Data.GyroY = (DataH << 8) | DataL;
	
	DataH = BNO055_ReadReg(BNO055_GYRO_ZOUT_H);
	DataL = BNO055_ReadReg(BNO055_GYRO_ZOUT_L);
	BNO055_Data.GyroZ = (DataH << 8) | DataL;
}

/**
 * @brief 初始化BNO055传感器
 * @return 成功返回0，失败返回错误代码
 */
//uint8_t BNO055_Init(void)
//{
//    uint8_t chip_id = 0;
//    
//    // 读取芯片ID验证通信
//    chip_id = BNO055_ReadReg(BNO055_CHIP_ID_ADDR);
//    
//    if(chip_id != 0xA0) {
//        // 芯片ID不匹配，通信失败
//        return 1;
//    }
//    
//    // 切换到配置模式（允许修改寄存器）
//    BNO055_WriteReg(BNO055_OPR_MODE_ADDR, CONFIGMODE);
//    Delay_ms(20);  // 等待模式切换完成
//    
//    // 重置传感器（可选）
//    // BNO055_WriteReg(BNO055_SYS_TRIGGER_ADDR, 0x20);
//    // Delay_ms(650);  // 等待重置完成
//    
//    // 设置电源模式为正常模式
//    BNO055_WriteReg(BNO055_PWR_MODE_ADDR, NORMAL);
//    Delay_ms(10);
//    
//    // 设置操作模式为NDOF（9自由度融合模式）
//    BNO055_WriteReg(BNO055_OPR_MODE_ADDR, NDOF);
//    Delay_ms(30);  // 等待模式切换完成
//    
//    return 0;  // 初始化成功
//}
